/*
2020.11.16
Author:ZCM
*/
#include "esc_uart.h"
#include "gripper.h"
#include "user_app.h"
#include "string.h"


GRIPPER_OBJECT GRIPPER1=
{
	.uart = &UART_0,
	.grip_state = OFFLINE,
	.grip_id = 1U,
	.series_num = 0U,
	.update_flag = false,
	.updated_member={0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U},
	.rx_buffer={0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U},
	.cotrol_object=&Device1_Control0x6000,
	.feedback_object=&Device1_FeedBack0x6020
};

GRIPPER_OBJECT GRIPPER2=
{
	.uart = &UART_1,
	.grip_state = OFFLINE,
	.grip_id = 1U,
	.series_num = 0U,
	.update_flag = false,
	.updated_member={0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U},
	.rx_buffer={0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U},
	.cotrol_object=(TOBJ6000*)&Device2_Control0x6040,
	.feedback_object=(TOBJ6020*)&Device2_FeedBack0x6060
};

GRIPPER_OBJECT GRIPPER3=
{
	.uart = &UART_2,
	.grip_state = OFFLINE,
	.grip_id = 1U,
	.series_num = 0U,
	.update_flag = false,
	.updated_member={0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U},
	.rx_buffer={0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U},
	.cotrol_object=(TOBJ6000*)&Device3_Control0x6080,
	.feedback_object=(TOBJ6020*)&Device3_FeedBack0x60A0
};

GRIPPER_OBJECT GRIPPER4=
{
	.uart = &UART_3,
	.grip_state = OFFLINE,
	.grip_id = 1U,
	.series_num = 0U,
	.update_flag = false,
	.updated_member={0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U},
	.rx_buffer={0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U,0U},
	.cotrol_object=(TOBJ6000*)&Device4_Control0x60C0,
	.feedback_object=(TOBJ6020*)&Device4_FeedBack0x60E0
};

GRIPPER_OBJECT *GRIP_OB_LIST[4]={&GRIPPER1, &GRIPPER2, &GRIPPER3, &GRIPPER4};


/*
  功能：扫描端口连接的夹爪，获取夹爪真实ID,获取夹爪系列号
  返回:已连接的夹爪数目
 */
uint8_t Scan_Connected_Gripper(void)
{
	uint8_t grip_ob_index;
	uint8_t read_reg_num = 1, grip_num=0;
	uint8_t id_index = 4, series_num_index = 3;

	/*获取实际夹爪ID号：夹爪对象的grip_id起始为1，通过以下操作查询到实际的夹爪ID写入grip_id,没有连接夹爪的夹爪对象grip_id为0
	 *夹爪ID号可以相同
	 */
	for(grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		for(uint8_t loop_ctrl=0; loop_ctrl<4; loop_ctrl++)
		{
			ReadGripperReg(GRIP_OB_LIST[grip_ob_index], GripID_addr, read_reg_num);//读取夹爪的ID
			delay_ms(20);
			if(GRIP_OB_LIST[grip_ob_index]->rx_buffer[id_index] == GRIP_OB_LIST[grip_ob_index]->grip_id)
			{
				memset(GRIP_OB_LIST[grip_ob_index]->rx_buffer, 0U, MODBUS_READ_LEN(read_reg_num));//重置缓存
				break;
			}else
			{
				GRIP_OB_LIST[grip_ob_index]->grip_id++;
				if(GRIP_OB_LIST[grip_ob_index]->grip_id == 5)
				{
					GRIP_OB_LIST[grip_ob_index]->grip_id = 0;
				}
			}
		}
	}
	/*获取真实夹爪ID后，查询夹爪的系列号*/
	for(grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
		{
			continue;
		}
		ReadGripperReg(GRIP_OB_LIST[grip_ob_index], SeriesNum_addr, read_reg_num);//读取夹爪的序列号
		delay_ms(20);
		if(GRIP_OB_LIST[grip_ob_index]->rx_buffer[series_num_index])
		{
			grip_num++;
			GRIP_OB_LIST[grip_ob_index]->series_num = (GRIP_OB_LIST[grip_ob_index]->rx_buffer[series_num_index] >> 4);
			memset(GRIP_OB_LIST[grip_ob_index]->rx_buffer, 0U, MODBUS_READ_LEN(read_reg_num));//重置缓存
		}
	}

	return grip_num;
}


/*
   功能：切换夹爪工作模式
 ControlState 0：	485模式
 ControlState 1：	IO模式
*/
uint8_t Gripper_mode_Config(uint8_t timerID)
{
	static bool temp_mode_Config = false;
	uint8_t grip_ob_index;

	if((bool)I_OMode0x6102.IO_ModeEnable != temp_mode_Config)
	{
//		SYSTIMER_StopTimer(timerID);
		SYSTIMER_Stop();
		for(grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
		{
			if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
			{
				continue;
			}
			WriteGripperReg(GRIP_OB_LIST[grip_ob_index], IOmodeEnable_addr, I_OMode0x6102.IO_ModeEnable);
		}
		delay_ms(20);//等待数据返回完毕，避免发生数据传输碰撞


		for(grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
		{
			if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
			{
				continue;
			}
			//写入夹爪Flash保存
			WriteGripperReg(GRIP_OB_LIST[grip_ob_index], WriteFlash_addr, ENABLE_WRITE);
		}
		delay_ms(3000);//写入夹爪Flash保存时间需要很长

		temp_mode_Config = I_OMode0x6102.IO_ModeEnable;
		if(!temp_mode_Config)
		{
//			SYSTIMER_StartTimer(timerID);
			SYSTIMER_Start();
		}
	}
	return temp_mode_Config;
}

uint8_t Gripper_Opreat(uint8_t grip_ob_index, uint8_t cmd_index)
{
	int16_t IO_params_int16[6]={0};
	uint8_t IO_params_uint8[12];
	int16_t start_address;
	uint8_t state;

	switch(GRIP_OB_LIST[grip_ob_index]->updated_member[cmd_index])
	{
		case Init_cmd:
			switch(GRIP_OB_LIST[grip_ob_index]->cotrol_object->Initialization)
			{
				case 1:
				case 165:
					state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], Init_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->Initialization);
					delay_ms(1000);
					break;
				default:
					break;
			}
			break;

		case Force_cmd:
			if((19 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->Force)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->Force< 101))
			{
				state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], Force_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->Force);
			}
			break;

		case Position_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->cotrol_object->Position< 1001)
			{
				state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], Position_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->Position);
			}
			break;

		case Speed_cmd:
			if((0 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->Speed)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->Speed< 101))
			{
				state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], Speed_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->Speed);
			}
			break;

		case AngleLo_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], AngleLo_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleLo);
			}
			break;

		case AngleHi_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				if((-161 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleHi)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleHi< 161))
				{
					state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], AngleHi_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleHi);
				}
			}
			break;

		case RotaSpeed_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				if((0 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationSpeed)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationSpeed< 101))
				{
					state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], RotaSpeed_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationSpeed);
				}
			}
			break;

		case RotaForce_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				if((0 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationForce)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationForce< 101))
				{
					state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], RotaForce_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationForce);
				}
			}
			break;

		case AngleRelat_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], AngleRelat_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleRelatively);
				break;
			}

		case StopRota_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				if(GRIP_OB_LIST[grip_ob_index]->cotrol_object->StopRotation< 3)
				{
					state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], StopRota_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->StopRotation);
				}
			}
			break;

		case ResetRotaVal_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)
			{
				switch(GRIP_OB_LIST[grip_ob_index]->cotrol_object->ResetRotationVal)
				{
					case 1:
					case 165:
						state=WriteGripperReg(GRIP_OB_LIST[grip_ob_index], ResetRotaVal_addr, GRIP_OB_LIST[grip_ob_index]->cotrol_object->ResetRotationVal);
						break;
					default:
						break;
				}
			}
			break;

		case ControlWord_cmd:
			if(GRIP_OB_LIST[grip_ob_index]->series_num == PGE_SERIES)//非旋转夹爪
			{
				if((0 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord< 5))
				{
					IO_params_int16[0] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Position;//load position
					IO_params_int16[1] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Force;//load force
					IO_params_int16[2] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Speed;//load speed
					Uint16ToUint8((uint16_t*)IO_params_int16, IO_params_uint8, 3);
					start_address = 0x0402+3*GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord;
					state=WriteGripperMultiReg(GRIP_OB_LIST[grip_ob_index], start_address, 3, IO_params_uint8);
				}
			}else//周五写到此处
			if(GRIP_OB_LIST[grip_ob_index]->series_num == RGI_SERIES)//旋转夹爪
			{
				if((0 <GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord)&&(GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord< 3))
				{
					IO_params_int16[0] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Position;//load position
					IO_params_int16[1] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Force;//load force
					IO_params_int16[2] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->Speed;//load speed
					IO_params_int16[3] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->AngleLo;//load Angle_low
					IO_params_int16[4] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationForce;//load rotation force
					IO_params_int16[5] = GRIP_OB_LIST[grip_ob_index]->cotrol_object->RotationSpeed;//load rotation speed
					Uint16ToUint8((uint16_t*)IO_params_int16, IO_params_uint8, 6);
					start_address = 0x03FF+6*GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord;
					state=WriteGripperMultiReg(GRIP_OB_LIST[grip_ob_index], start_address, 6, IO_params_uint8);
				}
			}
			break;

		case No_Update:
			GRIP_OB_LIST[grip_ob_index]->update_flag = false;
			break;

		default:
			break;
	}

	return state;
}


//uint8_t control_delay_time = 15;//经过验证，这是最小的控制延时，低于该时间工作异常
/*发送夹爪控制命令，同一个夹爪一次只能发送一条指令，要避免出现多条指令同时发送*/
uint8_t ControlGripper(uint8_t used_port_number)
{
	uint8_t return_state[4]={0};

	for(uint8_t cmd_index=0; cmd_index<12; cmd_index++)//index update_member[12]的下标
	{
		for(uint8_t grip_ob_index=0; grip_ob_index<4; grip_ob_index++)
		{
			if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
			{
				continue;
			}

			return_state[grip_ob_index] = Gripper_Opreat(grip_ob_index, cmd_index);
		}
		//等待所有串口返回完数据，如果超时则补发一次
		for(HW_ClearTimer();;)
		{
			delay_ms(1);
			if(Check_Free_Port_Number() == used_port_number)
			{
				break;
			}else if(HW_GetTimer()>10)
			{
				for(uint8_t index=0; index<4; index++)
				{
					if(return_state[index])
					{
						GRIP_OB_LIST[index]->uart->runtime->tx_busy = false;
						GRIP_OB_LIST[index]->uart->runtime->rx_busy = false;
						Gripper_Opreat(index, cmd_index);
					}else
					{
						continue;
					}
				}
				break;
			}
		}

		if(Is_Control_Update() == false)
		{
			//如果更新完毕则退出
			for(HW_ClearTimer();;)
			{
				if(Check_Free_Port_Number() == used_port_number)
				{
					break;
				}else if(HW_GetTimer()>10)
				{
					break;
				}
			}
			break;
		}
	}
	//退出后重置更新标志区
	Reset_Update_Member();
//	delay_ms(control_delay_time);//等待最后一个夹爪的控制指令返回值返回，避免发生数据传输碰撞，为了可靠性，这步不能省略
	return 0;
}

/*
 * 重置更新标志区
 *
 */
uint8_t Reset_Update_Member(void)
{

	for(uint8_t grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
		{
			continue;
		}

		memset(GRIP_OB_LIST[grip_ob_index]->updated_member, 0U, 12);//置位更新缓存区，为下一个周期做准备
		GRIP_OB_LIST[grip_ob_index]->uart->runtime->rx_busy = false;//接收完成标志置位，防止影响读反馈命令
	}

	return 0;
}


/*读取夹爪反馈参数*/
volatile uint32_t time_feedback=0;
//volatile bool ctrl_flag = false;
uint8_t Check_Gripper_Status(void)
{
	for(uint8_t grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
		{
			continue;
		}

		time_feedback = SYSTIMER_GetTime();
//		ctrl_flag = true;
		ReadGripperReg(GRIP_OB_LIST[grip_ob_index], FBstart_addr, FB_REGS_NUM);//读取反馈数据
	}

	return 0;
}
/*
 * 检查从站是否已经返回数据
 *返回数：等于连接的夹爪数时代表全部从站数据已经返回
 */
uint8_t Check_Free_Port_Number(void)
{
	uint8_t state_word=0;

	for(uint8_t grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
		{
			continue;
		}

		if(GRIP_OB_LIST[grip_ob_index]->uart->runtime->rx_busy == false)
		{
			state_word++;
		}
	}

	return state_word;
}

/*
 * 计算控制更新标志位的和
 *
 */
bool Is_Control_Update(void)
{
	bool update_sum=false;

	for(uint8_t grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
	{
		if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
		{
			continue;
		}

		update_sum |= GRIP_OB_LIST[grip_ob_index]->update_flag;
	}

	return update_sum;
}

//检查数据对象是否更新，该函数在sync0中断服务函数中被调用
void Check_PDO_Update(void)
{
	uint16_t write_io = 0U;

	if(I_OMode0x6102.IO_ModeEnable)//IO模式
	{
		//将4个数据对象的IO_ControlWord成员组合成Px_TMS_Outputs IO_BUS控制字write_io
		for(uint8_t grip_ob_index = 0; grip_ob_index< 4; grip_ob_index++)
		{
			if(GRIP_OB_LIST[grip_ob_index]->grip_id == 0)
			{
				continue;
			}

			write_io |= (uint16_t)(GRIP_OB_LIST[grip_ob_index]->cotrol_object->IO_ControlWord<<(grip_ob_index*2));
		}
		BUS_IO_Write(&Px_TMS_Outputs, write_io);//将第x个数据对象的IO_ControlWord成员写入对第x个夹爪输出(以盒子为参考)的管脚
	}
}

/*
grip_ob		需要操作的夹爪对象
rges_addr	读取的寄存器的起始地址
number		读取的寄存器的数量
*/
uint8_t ReadGripperReg(GRIPPER_OBJECT *grip_ob, uint16_t regs_start_addr, uint8_t number)
{
	static uint8_t cmd[8] = {0x00,0x03,0x00,0x00,0x00};
	uint16_t crc;
	uint8_t state;

	cmd[SLAVE_ADDR] = grip_ob->grip_id;
	cmd[ADDR_HI] = (regs_start_addr>>8)&0xff;
	cmd[ADDR_LO] = regs_start_addr&0xff;
	cmd[NUM_REG_HI] = (number>>8)&0xff;
	cmd[NUM_REG_LO] = number&0xff;
	crc = CRC16(cmd,6);
	cmd[6] = crc&0xff;
	cmd[7] = (crc>>8)&0xff;

	state = Uart_Write_Read(grip_ob->uart, cmd, sizeof(cmd), grip_ob->rx_buffer, MODBUS_READ_LEN(number));//0x03读多个寄存器指令
	grip_ob->grip_state = state ? OFFLINE:ONLINE;//state返回0表示夹爪在线，否在掉线
	Box_Infomation0x6100.CommunicationState = state ? grip_ob->grip_id:0x00;//state!=0,夹爪掉线，将掉线夹爪ID号反馈给主站
	return state;
}

/*
grip_ob		需要操作的夹爪对象
rges_addr	写入寄存器的地址
value		写入寄存器的值
*/
uint8_t WriteGripperReg(GRIPPER_OBJECT *grip_ob, uint16_t reg_addr, uint16_t value)
{
	static uint8_t cmd[8] = {0x00,0x06,0x00,0x00,0x00};
	uint16_t crc;
	uint8_t state;

	cmd[SLAVE_ADDR] = grip_ob->grip_id;
	cmd[ADDR_HI] = (reg_addr>>8)&0xff;
	cmd[ADDR_LO] = reg_addr&0xff;
	cmd[4] = (value>>8)&0xff;
	cmd[5] = value&0xff;
	crc = CRC16(cmd,6);
	cmd[6] = crc&0xff;
	cmd[7] = (crc>>8)&0xff;

	state = Uart_Write_Read(grip_ob->uart, cmd, sizeof(cmd), grip_ob->rx_buffer, RX_SIZE_8);//0x06写单个寄存器指令，写命令数据长度等于返回的数据长度8(包括CRC校验码)
	grip_ob->grip_state = state ? OFFLINE:ONLINE;//state返回0表示夹爪在线，否在掉线
	Box_Infomation0x6100.CommunicationState = state ? grip_ob->grip_id:0x00;//state!=0,夹爪掉线，将掉线夹爪ID号反馈给主站
	return state;
}

/*
start	读取地址
num		寄存器个数
value	写入寄存器的值
recv	返回的数据
*/
uint8_t WriteGripperMultiReg(GRIPPER_OBJECT *grip_ob, uint16_t regs_start_addr, uint8_t number, uint8_t *tx_buf)
{
	static uint8_t cmd[50] = {0x00, 0x10, 0x00, 0x00, 0x00};
	uint16_t crc;
	uint8_t state = 0xff;

	cmd[SLAVE_ADDR] = grip_ob->grip_id;
	cmd[ADDR_HI] = (regs_start_addr>>8)&0xff;
	cmd[ADDR_LO] = regs_start_addr&0xff;
	cmd[NUM_REG_HI] = (number>>8)&0xff;
	cmd[NUM_REG_LO] = number&0xff;
	cmd[BYTE_COUNT] = number*2;
	for(int i=0; i<number; i++)
	{
		cmd[BYTE_COUNT+2*i+1] = tx_buf[2*i];
		cmd[BYTE_COUNT+2*i+2] = tx_buf[2*i+1];
	}
	crc = CRC16(cmd,7+number*2);
	cmd[7+number*2] = crc & 0xff;
	cmd[7+number*2+1] = (crc>>8) & 0xff;

	state = Uart_Write_Read(grip_ob->uart, cmd, MODBUS_WRITE_LEN(number), grip_ob->rx_buffer, RX_SIZE_8);////0x10指令
	grip_ob->grip_state = state ? OFFLINE:ONLINE;//state返回0表示夹爪在线，否在掉线
	Box_Infomation0x6100.CommunicationState = state ? grip_ob->grip_id:0x00;//state!=0,夹爪掉线，将掉线夹爪ID号反馈给主站
	return state;
}

/*串口0回调函数，将接收到的数据映射到相应PDO里去*/
void Uart0_Receive_Callback(void)
{
	uint8_t fb_ob_index=0;

	/*操作码为0x03，排除其他操作命令返回数据接收中断的干扰；读寄存器个数为14个，排除读单个寄存器命令的干扰*/
	if((GRIP_OB_LIST[0]->rx_buffer[1] == 0x03)&&(GRIP_OB_LIST[0]->rx_buffer[2]>=28))
	{
		for(int fb_regs_addr=0; fb_regs_addr<FB_REGS_NUM; fb_regs_addr++)
		{
			if((fb_regs_addr==0x05)||(fb_regs_addr==0x06)||(fb_regs_addr==0x07)||(fb_regs_addr==0x0A)||(fb_regs_addr==0x0C))//去除不必要上传的寄存器
			{
				continue;
			}else
			{
				((uint8_t*)GRIP_OB_LIST[0]->feedback_object)[2*fb_ob_index+OBJ_OFFSET] = GRIP_OB_LIST[0]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET+1];
				((uint8_t*)GRIP_OB_LIST[0]->feedback_object)[2*fb_ob_index+OBJ_OFFSET+1] = GRIP_OB_LIST[0]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET];
				fb_ob_index++;
			}
		}
	}

}

void Uart1_Receive_Callback(void)
{
	uint8_t fb_ob_index=0;

	if((GRIP_OB_LIST[1]->rx_buffer[1] == 0x03)&&(GRIP_OB_LIST[1]->rx_buffer[2]>=28))
	{
		for(int fb_regs_addr=0; fb_regs_addr<FB_REGS_NUM; fb_regs_addr++)
		{
			if((fb_regs_addr==0x05)||(fb_regs_addr==0x06)||(fb_regs_addr==0x07)||(fb_regs_addr==0x0A)||(fb_regs_addr==0x0C))
			{
				continue;
			}else
			{
				((uint8_t*)GRIP_OB_LIST[1]->feedback_object)[2*fb_ob_index+OBJ_OFFSET] = GRIP_OB_LIST[1]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET+1];
				((uint8_t*)GRIP_OB_LIST[1]->feedback_object)[2*fb_ob_index+OBJ_OFFSET+1] = GRIP_OB_LIST[1]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET];
				fb_ob_index++;
			}
		}
	}
}

void Uart2_Receive_Callback(void)
{
	uint8_t fb_ob_index=0;

	if((GRIP_OB_LIST[2]->rx_buffer[1] == 0x03)&&(GRIP_OB_LIST[2]->rx_buffer[2]>=28))
	{
		for(int fb_regs_addr=0; fb_regs_addr<FB_REGS_NUM; fb_regs_addr++)
		{
			if((fb_regs_addr==0x05)||(fb_regs_addr==0x06)||(fb_regs_addr==0x07)||(fb_regs_addr==0x0A)||(fb_regs_addr==0x0C))
			{
				continue;
			}else
			{
				((uint8_t*)GRIP_OB_LIST[2]->feedback_object)[2*fb_ob_index+OBJ_OFFSET] = GRIP_OB_LIST[2]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET+1];
				((uint8_t*)GRIP_OB_LIST[2]->feedback_object)[2*fb_ob_index+OBJ_OFFSET+1] = GRIP_OB_LIST[2]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET];
				fb_ob_index++;
			}
		}
	}
}

void Uart3_Receive_Callback(void)
{
	uint8_t fb_ob_index=0;

	if((GRIP_OB_LIST[3]->rx_buffer[1] == 0x03)&&(GRIP_OB_LIST[3]->rx_buffer[2]>=28))
	{
		for(int fb_regs_addr=0; fb_regs_addr<FB_REGS_NUM; fb_regs_addr++)
		{
			if((fb_regs_addr==0x05)||(fb_regs_addr==0x06)||(fb_regs_addr==0x07)||(fb_regs_addr==0x0A)||(fb_regs_addr==0x0C))
			{
				continue;
			}else
			{
				((uint8_t*)GRIP_OB_LIST[3]->feedback_object)[2*fb_ob_index+OBJ_OFFSET] = GRIP_OB_LIST[3]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET+1];
				((uint8_t*)GRIP_OB_LIST[3]->feedback_object)[2*fb_ob_index+OBJ_OFFSET+1] = GRIP_OB_LIST[3]->rx_buffer[2*fb_regs_addr+MOD_READ_DATA_OFFSET];
				fb_ob_index++;
			}
		}
	}
}

